﻿#include "SerialTools.h"
#include <QDebug>
#include <QFile>
#include <QVariantList>
#include <QVariantMap>

SerialTools::SerialTools(int argc, char** argv, QObject* parent) : argc_(argc), argv_(argv), QObject(parent)
{
    m_com = new QSerialPort(this);
    connect(m_com, SIGNAL(readyRead()), this, SLOT(onReadReady()));

    m_ros_spin_thread = std::thread{std::bind(&SerialTools::rosSpinThread, this)};

    initBas();

    m_timerExec = new QTimer(this);
    m_timerExec->setInterval(200);
    m_timerExec->setSingleShot(false);
    connect(m_timerExec, SIGNAL(timeout()), this, SLOT(onTimerExec()));

    m_com->setPortName(QString("COM3"));
    m_com->setBaudRate(QSerialPort::Baud9600);
    m_com->setParity(QSerialPort::NoParity);
    m_com->setDataBits(QSerialPort::Data8);
    m_com->setStopBits(QSerialPort::OneStop);
    bool bOpen = m_com->open(QIODevice::ReadWrite);
    qDebug() << "openCom " << (bOpen ? "OK" : "NG");
}

SerialTools::~SerialTools()
{
    rclcpp::shutdown();
}

void SerialTools::write(unsigned char* data, int len)
{
    QByteArray ba((char*)data, len);
    if (nullptr != m_com && m_com->isOpen()) {
        m_com->write(ba);
    }
}

void SerialTools::setCom(const std::string& comName)
{
    qDebug() << "openCom: " << QString::fromStdString(comName);
    m_com->setPortName(QString::fromStdString(comName));
    m_com->setBaudRate(QSerialPort::Baud9600);
    m_com->setParity(QSerialPort::NoParity);
    m_com->setDataBits(QSerialPort::Data8);
    m_com->setStopBits(QSerialPort::OneStop);
    QMetaObject::invokeMethod(this, "openCom", Qt::QueuedConnection);
}

void SerialTools::openCom()
{
    bool bOpen = m_com->open(QIODevice::ReadWrite);
    qDebug() << "openCom " << (bOpen ? "OK" : "NG");
}

void SerialTools::closeCom()
{
    m_com->close();
}

void SerialTools::call_cmd(const std::string& cmdName)
{
    QMetaObject::invokeMethod(this, cmdName.c_str(), Qt::QueuedConnection);
}

// void SerialTools::reset()
// {
//     quint8 arrCmd[8] = {0};
//     m_cmder->getCmd("reset", arrCmd);

//     write(arrCmd, 8);
// }

// void SerialTools::pinch()
// {
//     quint8 arrCmd[8] = {0};
//     m_cmder->getCmd("pinch", arrCmd);

//     write(arrCmd, 8);
// }

// void SerialTools::loose()
// {
//     quint8 arrCmd[8] = {0};
//     m_cmder->getCmd("loose", arrCmd);

//     write(arrCmd, 8);
// }

// void SerialTools::stop()
// {
//     quint8 arrCmd[8] = {0};
//     m_cmder->getCmd("stop", arrCmd);

//     write(arrCmd, 8);
// }

// void SerialTools::startPeel(int nCm)
// {
//     quint8 arrCmd[8] = {0};
//     m_cmder->getCmd("startPeel", arrCmd);

//     write(arrCmd, 8);
// }

// void SerialTools::stopPeel()
// {
//     quint8 arrCmd[8] = {0};
//     m_cmder->getCmd("stopPeel", arrCmd);

//     write(arrCmd, 8);
// }

void SerialTools::rosSpinThread()
{
    rclcpp::init(argc_, argv_);
    m_robotCmdNode = std::make_shared<RobotCmdNode>(this);
    rclcpp::spin(m_robotCmdNode);
    rclcpp::shutdown();
}

void SerialTools::onReadReady()
{
    QByteArray ba = m_com->readAll();
    printBa(std::move(ba));
}

void SerialTools::initBas()
{
    {
        QString str = "A2 06 A1 00 00 01 73 65";
        QStringList list = str.split(" ", Qt::SkipEmptyParts);
        for (const auto& i : list) {
            quint8 u8 = i.toUInt(0, 16);
            m_baAllReset.append(u8);
        }
    }

    {
        QString str = "A2 06 A1 00 00 03 F2 A4";
        QStringList list = str.split(" ", Qt::SkipEmptyParts);
        for (const auto& i : list) {
            quint8 u8 = i.toUInt(0, 16);
            m_baSongKai.append(u8);
        }
    }

    {
        QString str = "A2 06 A1 00 00 08 B3 63";
        QStringList list = str.split(" ", Qt::SkipEmptyParts);
        for (const auto& i : list) {
            quint8 u8 = i.toUInt(0, 16);
            m_baJiaJin.append(u8);
        }
    }

    {
        QString str = "A2 06 A1 00 00 05 72 A6";
        QStringList list = str.split(" ", Qt::SkipEmptyParts);
        for (const auto& i : list) {
            quint8 u8 = i.toUInt(0, 16);
            m_baStop.append(u8);
        }
    }

    {
        QString str = "A2 06 A1 00 4C D4 86 3A";
        QStringList list = str.split(" ", Qt::SkipEmptyParts);
        for (const auto& i : list) {
            quint8 u8 = i.toUInt(0, 16);
            m_baStartBoPi.append(u8);
        }
    }

    {
        QString str = "A2 06 A1 00 00 06 32 A7";
        QStringList list = str.split(" ", Qt::SkipEmptyParts);
        for (const auto& i : list) {
            quint8 u8 = i.toUInt(0, 16);
            m_baStopBoPi.append(u8);
        }
    }

    {
        QString str = "D2 10 5A 00 00 08 10 00 03 00 02 03 FF 13 88 00 0F 00 3C 40 00 08 0A 93 40";
        QStringList list = str.split(" ", Qt::SkipEmptyParts);
        for (const auto& i : list) {
            quint8 u8 = i.toUInt(0, 16);
            m_baStop2Song.append(u8);
        }
    }

    {
        QString str = "D2 10 5A 00 00 08 10 00 03 00 02 00 00 13 88 00 0F 00 3C 40 00 08 0A A4 53";
        QStringList list = str.split(" ", Qt::SkipEmptyParts);
        for (const auto& i : list) {
            quint8 u8 = i.toUInt(0, 16);
            m_baSong2Stop.append(u8);
        }
    }

    {
        QString str = "D2 10 5A 00 00 08 10 00 05 00 01 03 FF 13 88 00 0F 00 3C 40 00 08 0A 10 81";
        QStringList list = str.split(" ", Qt::SkipEmptyParts);
        for (const auto& i : list) {
            quint8 u8 = i.toUInt(0, 16);
            m_baStop2Jin.append(u8);
        }
    }

    {
        QString str = "D2 10 5A 00 00 08 10 00 05 00 01 00 00 13 88 00 0F 00 3C 40 00 08 0A 27 92";
        QStringList list = str.split(" ", Qt::SkipEmptyParts);
        for (const auto& i : list) {
            quint8 u8 = i.toUInt(0, 16);
            m_baJin2Stop.append(u8);
        }
    }

    {
        QString str = "C2 10 5A 00 00 08 10 00 06 00 12 03 20 13 88 00 0F 00 64 C0 00 0C 08 B6 04";
        QStringList list = str.split(" ", Qt::SkipEmptyParts);
        for (const auto& i : list) {
            quint8 u8 = i.toUInt(0, 16);
            m_baStop2Fanzhuan.append(u8);
        }
    }

    {
        QString str = "C2 10 5A 00 00 08 10 00 C2 00 11 03 20 13 88 00 0F 00 64 C0 00 0C 08 77 94";
        QStringList list = str.split(" ", Qt::SkipEmptyParts);
        for (const auto& i : list) {
            quint8 u8 = i.toUInt(0, 16);
            m_baStop2Zhengzhuan.append(u8);
        }
    }

    {
        QString str = "B2 10 5A 00 00 08 10 00 C2 00 11 03 FF 13 88 00 0F 00 64 C0 00 0C 08 8C 65";
        QStringList list = str.split(" ", Qt::SkipEmptyParts);
        for (const auto& i : list) {
            quint8 u8 = i.toUInt(0, 16);
            m_baStop2Zhengzhuan_qiedao.append(u8);
        }
    }
}

void SerialTools::bpq_allReset()
{
    m_com->write(m_baAllReset);
}

void SerialTools::bpq_jiaJin()
{
    m_com->write(m_baJiaJin);
}

void SerialTools::bpq_songKai()
{
    m_com->write(m_baSongKai);
}

void SerialTools::bpq_stop()
{
    m_com->write(m_baStop);
}

void SerialTools::bpq_startBoPi()
{
    m_com->write(m_baStartBoPi);
}

void SerialTools::bpq_stopBoPi()
{
    m_com->write(m_baStopBoPi);
}

void SerialTools::jq_stop2song()
{
    printBa(std::move(m_baStop2Song));
    m_com->write(m_baStop2Song);
}

void SerialTools::jq_song2stop()
{
    m_com->write(m_baSong2Stop);
}

void SerialTools::jq_stop2jin()
{
    printBa(std::move(m_baStop2Song));
    m_com->write(m_baStop2Jin);
}

void SerialTools::jq_jin2stop()
{
    m_com->write(m_baJin2Stop);
}

void SerialTools::bs_fanzhuan()
{
    m_baQieDao_BanShou = m_baStop2Fanzhuan;
    m_timerExec->start();
}

void SerialTools::bs_zhenzhuan()
{
    m_baQieDao_BanShou = m_baStop2Zhengzhuan;
    m_timerExec->start();
}

void SerialTools::bs_stop()
{
    m_timerExec->stop();
}

void SerialTools::qd_zhenzhuan()
{
    m_baQieDao_BanShou = m_baStop2Zhengzhuan_qiedao;
    m_timerExec->start();
}

void SerialTools::qd_stop()
{
    m_timerExec->stop();
}

void SerialTools::onTimerExec()
{
    m_com->write(m_baQieDao_BanShou);
}

void SerialTools::printBa(const QByteArray&& ba)
{
    QString strLog;
    qDebug("tool com send:");
    for (int i=0; i<ba.size(); ++i)
    {
        strLog += QString::number(quint8(ba.at(i)), 16).toUpper().rightJustified(2, '0') + " ";
    }
    qDebug() << strLog;
}